// Ted Carancho's MikroQuad - November 2008
// An Arduino based quadrocopter using the Sparkfun
// 5DOF IMU and IDG300 Dual Axis Gyro
// This version will be able to use gyros for stability (acrobatic mode)
// or accelerometers (stable mode).

#include <EEPROM.h>
#include <ServoTimer2.h>

// ******************** Initialize Variables ********************
#define BAUD 38400

// Define sensor pin assignments
#define XACCELPIN 0
#define YACCELPIN 1
#define ZACCELPIN 2
#define PITCHRATEPIN 3
#define ROLLRATEPIN 4
#define YAWRATEPIN 5

// Define EEPROM storage
#define RATE_PGAIN_ADR 0
#define RATE_IGAIN_ADR 4
#define RATE_DGAIN_ADR 8
#define POS_PGAIN_ADR 12
#define POS_IGAIN_ADR 16
#define POS_DGAIN_ADR 20
#define XMITFACTOR_ADR 24


// Define transmitter commands
#define TIMEOUT 2500
#define THROTTLEPIN 4
#define ROLLPIN 2
#define PITCHPIN 3
#define YAWPIN 6
#define MODEPIN 7
#define AUXPIN 5
volatile int throttle = 0;
volatile int roll = 0;
volatile int pitch = 0;
volatile int yaw = 0;
volatile int mode = 0; // >1500 stable mode,  <1500 acrobatic mode
volatile int aux= 0;
int rollCommand = 1500;
int pitchCommand = 1500;
int yawCommand = 1500;
int rollCommandZero, pitchCommandZero, yawCommandZero;
// Controls the strength of the commands sent from the transmitter
// xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
float xmitFactor;


// Define motor commands
#define MINCOMMAND 1000
#define MAXCOMMAND 2000
#define FRONTMOTORPIN 8
#define REARMOTORPIN 9
#define RIGHTMOTORPIN 10
#define LEFTMOTORPIN 11
ServoTimer2 frontMotor;
ServoTimer2 rearMotor;
ServoTimer2 rightMotor;
ServoTimer2 leftMotor;
int frontCommand;
int rearCommand;
int rightCommand;
int leftCommand;
int rollMotorCommand = 1500;
int pitchMotorCommand = 1500;
int yawMotorCommand = 1500;

// Define sensor data
#define ZEROLIMIT 2
// Accel setup
int rollSensor, pitchSensor, yawSensor;
int xAccelZero, yAccelZero, zAccelZero;
int zAccSensor;
float rollLevelAdjust, pitchLevelAdjust;
float levelFactor = 1.0;

// Gyro setup
int rollGyroZero, pitchGyroZero, yawGyroZero;
// rate = (N-512)/1024 * 3.3V * (1 deg/s)/.002V * (Pi radians)/(180 deg)
// rate = (N-512)/1024 * (double) 28.783, where N is measured A/D value
float convert2Radians = 1 / 29473.792;

// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Scale gyro output (-465 to +465) to motor commands (1000 to 2000)
// use y = mx + b
float mMotorRate = 1.0753; // m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465))
float bMotorRate = 1500;   // b = y1 - m * x1

// Scale accel output (-100 to +100) to motor commands (1000 to 2000)
float mMotorRoll = 5;
float bMotorRoll = 1500;

// Scale accel output (+100 to -100) to motor commands (1000 to 2000)
float mMotorPitch = -2.5;
float bMotorPitch = 1500;

// Smoothing parameters
int rollSmoothArray [16];
int pitchSmoothArray [16];
int yawSmoothArray [16];

// PID Values
#define WINDUP_GUARD_GAIN 100.0
float pTerm, iTerm, dTerm; 
float pgain = 0;
float igain = 0;
float dgain = 0;
float rate_pgain = 0;
float rate_igain = 0;
float rate_dgain = 0;
float pos_pgain = 0;
float pos_igain = 0;
float pos_dgain = 0;
float iRollState = 0;
float lastRollPosition = 0;
float iPitchState = 0;
float lastPitchPosition = 0;
float iYawState = 0;
float lastYawPosition = 0;

// Kalman filter
struct Gyro1DKalman {
  float x_angle, x_bias;
  float P_00, P_01, P_10, P_11;	
  float Q_angle, Q_gyro;
  float R_angle;
};
struct Gyro1DKalman rollFilter;
struct Gyro1DKalman pitchFilter;
float dt;

// Communication
volatile char queryType = 'X';
char string[32];
byte armed;

// Interrupt Handler
volatile byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is connected to 3.3V
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  pinMode(MODEPIN, INPUT);
  pinMode(AUXPIN, INPUT);
  
  // Configure motors
  frontMotor.attach(FRONTMOTORPIN);
  rearMotor.attach(REARMOTORPIN);
  rightMotor.attach(RIGHTMOTORPIN);
  leftMotor.attach(LEFTMOTORPIN);
  commandAllMotors(MINCOMMAND);

  // Read PID values form EEPROM
  rate_pgain = readFloat(RATE_PGAIN_ADR);
  rate_igain = readFloat(RATE_IGAIN_ADR);
  rate_dgain = readFloat(RATE_DGAIN_ADR);
  pos_pgain = readFloat(POS_PGAIN_ADR);
  pos_igain = readFloat(POS_IGAIN_ADR);
  pos_dgain = readFloat(POS_DGAIN_ADR);

  // Read transmit factor from EEPROM
  xmitFactor = readFloat(XMITFACTOR_ADR);

  // Setup interrupt to trigger on pin D2 (roll receiver pin)
  attachInterrupt(0, interruptHandler, RISING);
  armed = 0;
  
  // Wait for ESC's to initialize
  delay(3000);

  // Calibrate sensors
  zeroGyros();
  zeroAccelerometers();
  initGyro1DKalman(&rollFilter, 0.0001, 0.0003, 0.3);
  initGyro1DKalman(&pitchFilter, 0.0001, 0.0003, 0.3);

  // Continue to wait for ESC's to initialize
  delay(2000);

  previousTime = millis();
}

// ******************** Main MikroQuadAero Loop ********************
void loop () {
  // Measure loop rate
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;
  
  // Send configuration commands from transmitter
  if (throttle < 1010) {
    // Disarm motors (throttle down, yaw left)
    if (yaw < (MINCOMMAND + 100) && armed == 1) {
      armed = 0;
      commandAllMotors(MINCOMMAND);
    }
    
    // Zero sensors (throttle down, yaw left, roll left)
    if (yaw < (MINCOMMAND + 100) && (roll > (MAXCOMMAND - 100))) {
      zeroGyros();
      zeroAccelerometers();
    }
    
    // Arm motors (throttle down, yaw right)  
    if (yaw > (MAXCOMMAND - 100) && armed == 0) {
      armed = 1;
      commandAllMotors(MINCOMMAND + 75);
      delay(500);
      commandAllMotors(MINCOMMAND);
    }
  }

  // Read Sensors
  // Average sensor values and center around zero
  rollSensor = smooth(analogRead(ROLLRATEPIN), rollSmoothArray) - rollGyroZero;
  pitchSensor = smooth(analogRead(PITCHRATEPIN), pitchSmoothArray) - pitchGyroZero;
  yawSensor = smooth(analogRead(YAWRATEPIN), yawSmoothArray) - yawGyroZero;

  if (mode < 1500) {
    // Acrobatic Mode
    rollLevelAdjust = 0;
    pitchLevelAdjust = 0;
  }
  else {
    // Stable Mode
    zAccSensor = analogRead(ZACCELPIN) + zAccelZero;
    dt = deltaTime / 1000.0;

    predictKalman(&rollFilter, rollSensor * convert2Radians, dt);
    rollLevelAdjust = updateKalman(&rollFilter, atan2(analogRead(YACCELPIN) + yAccelZero, zAccSensor)) * levelFactor;

    predictKalman(&pitchFilter, pitchSensor * convert2Radians, dt);
    pitchLevelAdjust = updateKalman(&pitchFilter, atan2(analogRead(XACCELPIN) + xAccelZero, zAccSensor)) * levelFactor;
  }
  
  // Calculate Commanded Angles
  // Reduce transmitter commands using xmitFactor and center around 1500
  rollCommand = ((roll - 1500) * xmitFactor) + 1500 + rollLevelAdjust;
  pitchCommand = ((pitch - 1500) * xmitFactor) + 1500 + pitchLevelAdjust;
  yawCommand = ((yaw - 1500) * xmitFactor) + 1500;
  
  // Update PID
  // Also convert sensor values to same range as PWM
  rollMotorCommand = updatePID((mMotorRate * rollSensor) + bMotorRate, rollCommand, &lastRollPosition, &iRollState);
  pitchMotorCommand = updatePID((mMotorRate * pitchSensor) + bMotorRate, pitchCommand, &lastPitchPosition, &iPitchState);
  yawMotorCommand = updatePID((mMotorRate * yawSensor) + bMotorRate, yawCommand, &lastYawPosition, &iYawState);   

  // Calculate motor commands
  frontCommand = constrain((throttle - pitchMotorCommand + yawMotorCommand), MINCOMMAND, MAXCOMMAND);
  rearCommand = constrain((throttle + pitchMotorCommand + yawMotorCommand), MINCOMMAND, MAXCOMMAND);
  rightCommand = constrain(throttle - rollMotorCommand - yawMotorCommand, MINCOMMAND, MAXCOMMAND);
  leftCommand = constrain((throttle + rollMotorCommand - yawMotorCommand), MINCOMMAND, MAXCOMMAND);
    
  // Command motors
  if (armed) {
    frontMotor.write(frontCommand);
    rearMotor.write(rearCommand);
    rightMotor.write(rightCommand);
    leftMotor.write(leftCommand);
  }
  
  // Check for serial message
  if (Serial.available()) {
    queryType = Serial.read();
    if (queryType == 'U') {
      // Update PID values    
      rate_pgain = readFloatSerial();
      writeFloat(rate_pgain, RATE_PGAIN_ADR);
      rate_igain = readFloatSerial();
      writeFloat(rate_igain, RATE_IGAIN_ADR);
      rate_dgain = readFloatSerial();
      writeFloat(rate_dgain, RATE_DGAIN_ADR);
      pos_pgain = readFloatSerial();
      writeFloat(pos_pgain, POS_PGAIN_ADR);
      pos_igain = readFloatSerial();
      writeFloat(pos_igain, POS_IGAIN_ADR);
      pos_dgain = readFloatSerial();
      writeFloat(pos_dgain, POS_DGAIN_ADR);
      // Update transmitter factor
      xmitFactor = readFloatSerial();
      writeFloat(xmitFactor, XMITFACTOR_ADR);
      queryType = 'X';
    }
  }
}

// ******************** Subroutines ********************
// Used to read floating point values from the serial port
float readFloatSerial() {
  int i = -1;
  char data[128] = "";
  
  do {
    while (Serial.available() == 0) delay(50); // wait if no data
    i++;
    data[i] = Serial.read();
  } while (data[i] != ';'); // read serial port until semi-colon found
  
  return strtod(data, NULL);
}

// Sends commands to all motors
void commandAllMotors(int motorCommand) {
  frontMotor.write(motorCommand);
  rearMotor.write(motorCommand);
  rightMotor.write(motorCommand);
  leftMotor.write(motorCommand);
}
